#ifndef PATHSROUTING_H 
#define PATHSROUTING_H

#include <ros/ros.h>
#include <vector>
#include <utility>
#include <iostream>
#include <thread>
#include <memory>
#include <chrono>
#include <nav_msgs/Path.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/LaserScan.h>
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseArray.h>
#include <tf/tf.h>
#include <tf2/utils.h>
#include "paths_struct.h"
#include "paths_routing_par.h"
#include "match_projection_point.h"
#include "cached_path_profile.h"
#include "CubicSpline2D.h"
#include "FrenetOptimalTrajectory.h"
#include "FrenetPath.h"
#include "trajectoryPoint.h"
#include "cartesian_to_frenet.h"
#include "Obstacles.h"


class Paths_Routing
{
public:

    Paths_Routing();
    ~Paths_Routing();
    void Global_Smooth_Paths(const nav_msgs::Path::ConstPtr& global_paths_msg);
    void Odom(const nav_msgs::Odometry::ConstPtr& odom_msg);
    void vehicle_outline();
    void GoalPose(const geometry_msgs::PoseStamped::ConstPtr& goal_msg);
    void Scan(const sensor_msgs::LaserScan::ConstPtr& scan_msg);
    void SegmentInterceptThread(void);
    bool paths_segment_intercept(int proj_index , CachedPathProfile &cache);
    void PathsRoutingThread(void);
    void joinThread(std::unique_ptr<std::thread>& th);
    FrenetHyperparameters createFrenetHyperparameters();
    FrenetInitialConditions createFrenetInitialConditions();
    bool is_collision(std::unique_ptr<FrenetPath>& fts, const double& COLLISION_CHECK,
                                                            const std::vector<Point>& obstacles);
    void FrenetPathsPub(const DiscretizedTrajectory& best_path);
    int is_update_dynamic(nav_msgs::Path &trj_point_array, nav_msgs::Odometry &odom, int size);
    double dotProduct(const Point& a, const Point& b);
    double magnitude(const Point& a);
    double angleBetween(const Point& A, const Point& B, const Point& C);

private:

    double goal_pose_x, goal_pose_y, goal_pose_yaw;
    double init_lon_state;
    double ds0;
    double dds0;
    double d0;
    double dd0;
    double ddd0;
    double best_ds0;
    double init_relative_time;   // 规划起始点的时间

    int last_vehicle_index;
    int num_next;

    bool is_goal_receive = false;
    bool is_global_s_heading_ = false;
    bool is_path_segment;
    bool is_paths_finish = true;
    bool local_finish = true;
    bool messege2 = false;
    bool messege1;
    std::vector<std::pair<double, double>> global_paths;

    ros::NodeHandle nh;
    ros::Subscriber global_smooth_paths_;
    ros::Subscriber odom_;
    ros::Subscriber goal_pose_;
    ros::Subscriber scan_;

    ros::Publisher vehicle_outline_pub_;
    ros::Publisher local_paths_pub_;
    ros::Publisher local_paths_k;
    ros::Publisher local_paths_t;
    ros::Publisher local_paths_a;
    ros::Publisher local_paths_v;
    ros::Publisher Start_Dynamic;

    std_msgs::String start_dynamic;  // 判断局部规划是否开始
    nav_msgs::Odometry odom;
    nav_msgs::Path traj_points_;  
    geometry_msgs::PoseArray pubLocalPath_a;
    geometry_msgs::PoseArray pubLocalPath_t;
    geometry_msgs::PoseArray pubLocalPath_k;
    geometry_msgs::PoseArray pubLocalPath_v;

    std::unique_ptr<std::thread> segment_intercept_thread_;
    std::unique_ptr<std::thread>  paths_routing_thread_;
    std::unique_ptr<CubicSpline2D> CubSpline2;
    std::unique_ptr<FrenetPath> best_frenet_path;

    PointAndIndex projection_point;
    Paths_Routing_Par prp;
    PathsSegmentCached  PathSegment;
    Match_Projection_Point mpp;
    CachedPathProfile cache;
    FrenetOptimalTrajectory fot;
    CartesianToFrenet cft;
    Obstacles obs;
    
};

#endif // PATHSROUTING_H